/**
*  \file control.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "control.h"
#include "../assertor/assertor.h"
#include "../pal/pal_mem.h"
#include "../parameter/parameter.h"
#include "../control_fan/control_fan.h"
#include "../contactor/contactor.h"
#include "../mlib/mlib.h"
#include "../driver_present/driver_present.h"
#include "../control_pump/control_pump.h"
#include "../timerecorder/timerecorder.h"
#include "../nth/nth.h"
#include "../control_buzzer/control_buzzer.h"
#include "../control_iso_valve/control_iso_valve.h"
#include "../control_main_contactor/control_main_contactor.h"
#include "../control_dcdc/control_dcdc.h"
#include "../control_precharge/control_precharge.h"
#include "../control_traction/control_traction.h"
#include "../control_traction/traction.h"
#include "../supervisor/supervisor.h"
#include "../supervisor/supervisor_cfg.h"
#include "../lithium_ion/li_ion.h"                  /* app_li_ion_initialize(), ..        */
#include "../lithium_ion/li_ion_pred_bat.h"         /* li_ion_pred_ibat_initialize(), ..  */
#include "../cfg/service_cfg.h"
#include "../bdi/bdi.h"
#include "../cancom/cancom.h"
/*===============================================[ private defines   ]================================================*/
#define  DCDC_DELAY_TIMER  200
/*===============================================[ private datatypes ]================================================*/

/** Private variables */
struct control_private
{
    struct control_fan_obj fan1; /**<  */
    struct control_fan_obj fan2; /**<  */
};

/*===============================================[ private variables ]================================================*/
static struct control_private control;


/*===============================================[ private functions ]================================================*/


BO output_1_0Hz_Blinker_Management_10ms(VO);
BO control_led(S16 keyswitch);
BO control_inverter(BO golabl_enable);
void adc_debug_can_output(struct input_data_obj const * in, struct output_data_obj * out );


/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/
RC control_initialize( void )
{
    pal_memzero( &control, sizeof control);

    control_fan_construct( &control.fan1, &service->fan1_temperature_on, &service->fan1_temperature_off);
    control_fan_construct( &control.fan2, &service->fan2_temperature_on, &service->fan2_temperature_off);
    control_buzzer_initialize();
    control_iso_valve_initialize();
    control_main_contactor_initialize();
    control_dcdc_initialize();
    control_precharge_initialize();
    control_traction_initialize();
    control_pump_initialize();
    timerecorder_initialize();

    return RC_SUCCESS;
}

void control_everything( struct input_data_obj const * in, struct output_data_obj * out )
{
    ASSERTOR_PRE( NIL != in ,"NIL not expected");
    ASSERTOR_PRE( NIL != out,"NIL not expected");

	static U32 temp = 38;
	
    if( in->reserve_input>10 )
    {
        volatile BO x=TRUE;
        x=FALSE;
    }

    out->fan_motor             = control_fan( &control.fan1, in->temperature_motor_2 );
    out->fan_controller        = control_fan( &control.fan2, in->temperature_motor_1 );    
    out->buzzer                = control_buzzer_10ms( in->direction, service->buzzer_on_if_reverse  );
    out->alarm_buzzer          = control_alarm_buzzer_10ms(fBuzz4);
    out->main_contactor_pwm    = control_main_contactor_10ms(!fDisAll,pR.complete_charge, in->adc_voltage[mcu_adc_DcLink], in->adc_voltage[mcu_adc_KeySwitch]) ;  //fDisAll
    out->iso_valve_pwm         = control_iso_valve_10ms( out->main_contactor_pwm>0, in->driver_present, in->adc_voltage[mcu_adc_KeySwitch] );  //iso_valve
    out->precharge             = precharge_10ms(in->adc_voltage[mcu_adc_DcLink],in->adc_voltage[mcu_adc_KeySwitch]);
    out->pump_motor_rpm		   = control_pump_10ms();
    out->pump_motor_torque     = calculate_pump_torque_10ms();
    out->mR_mSet_To_LAC        = 0;         /* traction Torque_Out */
	out->mL_mSet_To_LAC        = control_traction_10ms( in, out );
    out->dcdc_control		   = control_dcdc_10ms(in->key_switch,out->precharge, in->adc_voltage[mcu_adc_DcLink], in->adc_voltage[mcu_adc_KeySwitch],DCDC_DELAY_TIMER);
    out->led                   = control_led(in->key_switch);
    out->inverter_1_enable     = control_inverter(!fDisAll);
    out->inverter_2_enable     = control_inverter(!fDisAll);
    out->reset_error_cpu       = (in->fan_error>0)||(in->main_contactor_error>0)||(in->iso_valve_error>0)||(in->boot_anchor_cpu_m>0); /* this is only a test an no function  */
   // out->pump_motor_torque  = 0; 
    out->fH16 = 0;                                   /* H16 boot lid contact open  */  
    if((out->main_contactor_pwm > 1000)&&(in->adc_voltage[mcu_adc_KeySwitch]-in->adc_voltage[mcu_adc_DcLink] <2 ))
    {
 	   out->main_contactor_flag = 1;
    }
    else
    {
 	   out->main_contactor_flag = 0;
    }

	out->discharge_flag =pR.discharge_flag;
	if(pR.discharge_flag ==1)
	{
		out->charge_state = discharging;
	}
	else if(pR.complete_charge ==1)
	{
		out->charge_state = compCharhed;
	}
	else if(pR.start_charge ==1)
	{
		out->charge_state = charging;
	}
	out->battery = 100;
	if( out->main_contactor_flag == 0)
	{
		out->mL_CmdPwmEna_1 = dis_ctrl.trac_PwmEna;
		out->mL_CmdEnable = dis_ctrl.trac_CmdEnable;
		out->mL_discharge = dis_ctrl.trac_discharge;

	    out->pwmEnabl = dis_ctrl.pump_pwmEnabl;
	    out->L_Enable = dis_ctrl.pump_Enable; 
	    out->pump_motor_discharge = dis_ctrl.pump_motor_discharge;
	}
	else
	{
	    /* traction  enable*/
		out->mL_CmdPwmEna_1 = (BO)Traction.Output.Enable_Left_Motor_Pwm;
		out->mL_CmdEnable   = (BO)Traction.Output.Enable_Left_Motor;;
		out->mL_discharge   = 0;

        /*pump enable   */
		out->pwmEnabl = pump_motor_PWM_enable;
		out->L_Enable = pump_motor_enable; 
		out->pump_motor_discharge = 0;

	}
    
	/*  directly  from input  */
    out->ou_direction          =  in->direction;
    out->Bat_voltage        =  in->adc_voltage[mcu_adc_KeySwitch];

	/*
   	if(BAT_TYPE_LEADACID == service->batterytype)
	{
		out->Bat_voltage    = BDI_VBatt * 100.0f;	// 10mv perBit
	}
	else
	{
		//To be implement!	Get from BMS
		out->Bat_voltage    =  in->adc_voltage[mcu_adc_KeySwitch];
	}
    */

	
    out->ou_driver_present     =  in->driver_present;
    out->trac_temperature      =  in->temperature_motor_1;
    out->ou_park_break_switch  =  (input_data.park_break_switch>5?1:0 );
    out->ou_seat_switch_1      =  (input_data.seat_switch_1>5?1:0);
    /*  supervisor call here */
    out->mLR_CmdHBeatCnt++;
	/*  dsc  */ 
    out->ou_fRedu1          =  fRedu1;         /* speed reduction switch      */ 
    out->ou_fRedu2          =  fRedu2;
    out->ou_fLampYel        =  fLampYel;       /* motor over temperature  the  yellow  led on  */
    out->ou_fLampRed        =  fLampRed;       /* motor overtemp. red         */
    out->ou_fBlinkBrk       =  fBlinkBrk;
    out->ou_fDeepDis        =  fDeepDis;       /* deep-discharge              */
    out->ou_fLampErr        =  fLampErr;       /* error lamp                  */  
    out->ou_fLampBook       =  fLampBook;      /* H3 on if DTC present or demanded by application */;  
    out->ou_fLampH7         =  fLampH7;
    out->ou_fBlinkH7        =  fBlinkH7;       /* H7  neutral yellow signal   */ 
    out->ou_fBlinkSA        =  fBlinkSA; 
    //out->ou_in_bBelt        =  in_bBelt;
    out->ou_in_bHBrake_N    =  in_bHBrake_N;

	out->disable_Trac       = fDisTrac;                   /* disable traction */
	out->disable_pump       = fDisPump;                   /* disable pump     */

    out->ou_tr_vSen         =  Traction.Output.Truck_Speed_mps;          /* truck actual speed unit m/s*/
    out->te_vSen            =  Traction.Output.Truck_Speed_mph *0.001f;  /* truck actual speed unit km/h*/
	
    app_li_ion_10ms();       /* 10ms-application of lithium-ion-battery managment                    */
    supervisor();
	te_Conv2Diagno();
   
	/* debug test*/ 
    adc_debug_can_output(in, out );
	//--------------------------------		
    out->ou_bBlink1_0Hz = output_1_0Hz_Blinker_Management_10ms();

   /* calculating times */
    {
        timerecoder_count( timerecoder_channel_component_work_time, TRUE                        );
        timerecoder_count( timerecoder_channel_driver_present,      in->driver_present          );
        timerecoder_count( timerecoder_channel_fan1,                out->fan_motor              );
        timerecoder_count( timerecoder_channel_fan2,                out->fan_controller         );
        timerecoder_count( timerecoder_channel_forward_switch,      in->direction==direction_switch_state_forward );
        timerecoder_count( timerecoder_channel_neutral_switch,      in->direction==direction_switch_state_neutral );
        timerecoder_count( timerecoder_channel_reverse_switch,      in->direction==direction_switch_state_reverse );
        timerecoder_count( timerecoder_channel_traction_forward,    in->truck_speed>50 );
        timerecoder_count( timerecoder_channel_traction_reverse,    in->truck_speed<-50);

        timerecorder_cyclic_service();
    }
   /* lead-acid battery-task */
	if(BAT_TYPE_LEADACID == service->batterytype)
	{
		BDI_Meas_task();   			
	}
   
};

void control_idle_service(void )
{
    timerecorder_idle_service();
}

/*===============================================[ private functions ]================================================*/

BO control_led(S16 keyswitch)
{
   static BO  rv = FALSE;
    static U16 count=0;

    if(keyswitch > 5)
    {
    	NTH(&count,99);   

		rv = (count<50) ;
		
	}
	else if(keyswitch < -5)
	{
		rv = FALSE;
	}
	
	return rv;
}
BO output_1_0Hz_Blinker_Management_10ms(VO)
{
   F32 static ti;
   BO  blink;
   if      (ti < 0.000f)   {  ti  = 0.0f;    blink = 0;  }
   else if (ti < 0.500f)   {  ti += TS;      blink = 0;  }
   else if (ti < 1.000f)   {  ti += TS;      blink = 1;  }
   else                    {  ti  = 0.0f;    blink = 0;  }
   return blink;

}

BO control_inverter(BO golabl_enable)
{
	return golabl_enable;

}

void adc_debug_can_output(struct input_data_obj const * in, struct output_data_obj * out )
{


    #if 0
	{/* debug 0  */
		 out->v_debug_0[0]= in->temperature_motor_1;
		 out->v_debug_0[1]= in->adc_voltage[mcu_adc_TempMotInv1];
		 out->v_debug_0[2]= in->temperature_motor_2;
		 out->v_debug_0[3]= in->adc_voltage[mcu_adc_TempMotInv2];

	   	/*//debug 1
		out->v_debug_1[0]= in->adc_voltage[mcu_adc_13_7VSupply]*10;
		 out->v_debug_1[1]= in->adc_voltage[mcu_adc_SteerAngleB]*10;
		 out->v_debug_1[2]= in->adc_voltage[  mcu_adc_lift_Pot ]*10;
		 out->v_debug_1[3]= in->adc_voltage[mcu_adc_TempMotInv1]*10;
		 */


		 out->v_debug_1[0]= pR.complete_charge;
		 out->v_debug_1[1]= pR.discharge_flag;
		 out->v_debug_1[2]= pR.start_charge;


	   	/* debug 2  */
		 out->v_debug_2[0]= in->adc_voltage[mcu_adc_CurrentCutOffIso]*10;
		 out->v_debug_2[1]= in->adc_voltage[mcu_adc_Accelerator2]*10;
		 out->v_debug_2[2]= in->adc_voltage[mcu_adc_CurrentMainCont]*10;
		 out->v_debug_2[3]= in->temperature_motor_2;

	   	/* debug 3  */
		 out->v_debug_3[0]= in->adc_voltage[mcu_adc_SteerAngleA]*10;
		 out->v_debug_3[1]= in->adc_voltage[mcu_adc_KeySwitch]*10;
		 out->v_debug_3[2]= in->adc_voltage[mcu_adc_12VSupply ]*10;
		 out->v_debug_3[3]= 0x00;

    }
	#endif
    #if 0
	{/* debug 0  */
		 out->v_debug_0[0]= in->temperature_motor_1;
		 out->v_debug_0[1]= in->adc_voltage[mcu_adc_5VExtSns]*10;
		 out->v_debug_0[2]= in->adc_voltage[mcu_adc_DcLink]*10;
		 out->v_debug_0[3]= in->adc_voltage[mcu_adc_Accelerator1]*10;

	   	/*//debug 1
		out->v_debug_1[0]= in->adc_voltage[mcu_adc_13_7VSupply]*10;
		 out->v_debug_1[1]= in->adc_voltage[mcu_adc_SteerAngleB]*10;
		 out->v_debug_1[2]= in->adc_voltage[  mcu_adc_lift_Pot ]*10;
		 out->v_debug_1[3]= in->adc_voltage[mcu_adc_TempMotInv1]*10;
		 */


		 out->v_debug_1[0]= pR.complete_charge;
		 out->v_debug_1[1]= pR.discharge_flag;
		 out->v_debug_1[2]= pR.start_charge;

		
	   	/* debug 2  */
		 out->v_debug_2[0]= in->adc_voltage[mcu_adc_CurrentCutOffIso]*10;
		 out->v_debug_2[1]= in->adc_voltage[mcu_adc_Accelerator2]*10;
		 out->v_debug_2[2]= in->adc_voltage[mcu_adc_CurrentMainCont]*10;
		 out->v_debug_2[3]= in->temperature_motor_2;
		 
	   	/* debug 3  */
		 out->v_debug_3[0]= in->adc_voltage[mcu_adc_SteerAngleA]*10;
		 out->v_debug_3[1]= in->adc_voltage[mcu_adc_KeySwitch]*10;
		 out->v_debug_3[2]= in->adc_voltage[mcu_adc_12VSupply ]*10;
		 out->v_debug_3[3]= 0x00;
		
    }
	#endif
	#if 0
	{
		/* debug 0  */
		 out->v_debug_0[0]= ((in->lift_switch_1 > 1)? 1:0);
		 out->v_debug_0[1]= ((in->lift_switch_2 > 1)? 1:0);
		 out->v_debug_0[2]= ((in->hyd_aux_switch_1 > 1)? 1:0);
		 out->v_debug_0[3]= ((in->tilt_switch  > 1)? 1:0);

	   	/* debug 1  */
		 out->v_debug_1[0]= ((in->key_switch    > 1)? 1:0);
		 out->v_debug_1[1]= ((in->seat_switch_1 > 1)? 1:0);
		 out->v_debug_1[2]= ((in->seat_switch_2 > 1)? 1:0);
		 out->v_debug_1[3]= ((in->park_break_switch > 1)? 1:0);
		
	   	/* debug 2  */
		 out->v_debug_2[0]= ((in->main_contactor_error > 1)? 1:0);
		 out->v_debug_2[1]= ((in->iso_valve_error > 1)? 1:0);
		 out->v_debug_2[2]= ((in->buzzer_error > 1)? 1:0);
		 out->v_debug_2[3]= ((in->fan_error > 1)? 1:0);
		 
	   	/* debug 3  */
		 out->v_debug_3[0]= in->temperature_motor_1;
		 out->v_debug_3[1]= in->adc_voltage[mcu_adc_TempMotInv1];
		 out->v_debug_3[2]= in->temperature_motor_2;
		 out->v_debug_3[3]= in->adc_voltage[mcu_adc_TempMotInv2];
	}
	#endif
	#if 0
	{
		/* debug 0  */
		 out->v_debug_0[0]= ((in->pedal_type == SINGLE_PEDAL)? 1:0);
		 out->v_debug_0[1]= out->Bat_voltage;
		 out->v_debug_0[2]= out->fan_motor;
		 out->v_debug_0[3]= out->fan_motor;

	   	/* debug 1  */
		 out->v_debug_1[0]= out->main_contactor_pwm;
		 out->v_debug_1[1]= out->precharge;
		 out->v_debug_1[2]= out->inverter_1_enable;
		 out->v_debug_1[3]= out->inverter_2_enable;
		
	   	/* debug 2  */
		 out->v_debug_2[0]= out->dcdc_control;
		 out->v_debug_2[1]= 0;
		 out->v_debug_2[2]= 0x00;
		 out->v_debug_2[3]= 0x00;
		 
	   	/* debug 3  */
		 out->v_debug_3[0]= 0x00;
		 out->v_debug_3[1]= 0x00;
		 out->v_debug_3[2]= 0x00;
		 out->v_debug_3[3]= 0x00;				
	}
	#endif
	
    #if 1 //xie
            /* debug 0  */
 	    out->v_debug_0[0] = out->reset_error_cpu;
	    out->v_debug_0[1] = input_data.fan_error;
	    out->v_debug_0[2] = input_data.main_contactor_error;
	    out->v_debug_0[2] = input_data.iso_valve_error;
        out->v_debug_1[0]= input_data.boot_anchor_cpu_m;
		 
	
	    //out->v_debug_1[0] = input_data.adc_voltage[mcu_adc_KeySwitch];
	    out->v_debug_1[1] = input_data.adc_voltage[mcu_adc_DcLink];
        out->v_debug_1[2] = out->ou_tr_vSen;  // m/s
        out->v_debug_1[3] = out->te_vSen /1000.0f;;    // m/h

		//out->ou_tr_vSen 		=  Traction.Output.Truck_Speed_mps; 		 /* truck actual speed unit m/s*/
		//out->te_vSen			=  Traction.Output.Truck_Speed_mph; 		 /* truck actual speed unit m/h*/

		


		out->v_debug_2[0]= ((fDisAll >= 1)? 1:0); 
		out->v_debug_2[1]= ((fDisPump>= 1)? 1:0);
		out->v_debug_2[2]= ((fDisTrac >= 1)? 1:0);

		out->v_debug_3[0]=  BDI_BattCharge;
		out->v_debug_3[1]=  service->batterytype;
		out->v_debug_3[2]=  output_data.te_vSen*100;
		out->v_debug_3[3]=  service->truck_speed_threshold;



		

         
#endif	
	
}









/***********************************************************************************************************************
*                             Copyright 2019 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/
